#include "main.h"
#include "control.h"
#include "can_receive.h"
#include "remote_control.h"
#include <math.h>
#include "mecanum.h"

int turn = 0;
int16_t totalangle=0;
float current_ecd=0;
float last_ecd=0;
float current_speed_rpm = 0;

PID_Controller pid_motor_position;//位置环初始化
PID_Controller pid_motor_speed;//速度环初始化
PID_Controller pid_mecanum[4];//麦克纳姆四轮


void PID_position_init(PID_Controller *pid, float Kp, float Ki, float Kd, float setpoint)
{
    pid->Kp = Kp;//P值
    pid->Ki = Ki;//I值
    pid->Kd = Kd;//D值
    pid->setpoint = setpoint;
    pid->integral = 0.0;
    pid->prev_error = 0.0;
    pid->last_error = 0.0;
}

float PID_position_calc(PID_Controller *pid, float measured_value,float setpoint) //位置式PID,角度环
{
    float error,derivative; // 计算误差
    error = setpoint - measured_value;
    pid->integral += error;                       // 更新积分项（偏差累积）

    if(pid->integral > 4000) pid->integral = 4000;
    if(pid->integral < -4000) pid->integral = -4000;

    derivative = error - pid->prev_error;   // 计算微分项
    pid->prev_error = error;                      // 更新上一次误差

    if(pid->integral>15000) pid->integral = 15000;
    if(pid->integral<-15000) pid->integral = -15000;
    
    float output = pid->Kp * error +
                   pid->Ki * pid->integral +
                   pid->Kd * derivative; // 计算PID输出

    
    return output;
}



float PID_plus_calc(PID_Controller *pid, float measured_value) //增量式PID
{
    float error; // 计算误差
    error = pid->setpoint - measured_value;
               
    float output = pid->Kp * (error - pid->last_error) +
                   pid->Ki * error +
                   pid->Kd * (error -2.0f*pid->last_error+pid->prev_error); // 计算PID输出

    pid->prev_error = pid->last_error;
    pid->last_error = error; // 更新上一次误差
    return output;
}

float PID_cascade_calc(PID_Controller *pid_inner,PID_Controller *pid_outer,int16_t outer_feedback,int16_t inner_feedback)
{
    float outer_output,inner_output,pid_inner_target;
    outer_output = PID_position_calc(pid_outer,outer_feedback,pid_outer->setpoint);
    pid_inner->setpoint = outer_output;// 这一步是将外环的输出反馈值给到内环目标值
    inner_output = PID_position_calc(pid_inner,(float)inner_feedback,pid_inner->setpoint);

    if (inner_output > 10000)   {inner_output = 10000;}
    if(inner_output < -10000)   {inner_output = -10000;}

    return inner_output;
}

// int16_t Angle_Control(int16_t Num,PID_Controller *speed,PID_Controller *position)//角度控制
// {
//     short motor_current = 0;

//     current_ecd = get_chassis_motor_measure_point(Num)->ecd;
//     current_speed_rpm = get_chassis_motor_measure_point(Num)->speed_rpm;

//     if(fabs(current_ecd-last_ecd)>4000)//突变，视为重装载了
//     {
//       if(current_ecd-last_ecd>0)//逆转了一圈
//       {
//         turn-=1;
//       }
//       else if(current_ecd-last_ecd<0)//正转了一圈
//       {
//         turn+=1;
//       }
//     }

//     totalangle=current_ecd+turn*8192;
//     motor_current = (int16_t)(PID_cascade_calc(&speed,&position,totalangle,current_speed_rpm));
//     last_ecd = current_ecd;

//     return motor_current;
// }

const PID_Controller *get_mecanum(uint8_t i)
{
  return &pid_mecanum[i];
}

void Mec_Control(PID_Controller *Mec)
{
    int16_t mec_current[4] = {0};
    int16_t current_speed[4] = {0};
    // Mec_Cal(&mec_chassis,&mec_wheel);
    
    for(int i = 0;i<4;i++)
    {
       current_speed[i]=get_chassis_motor_measure_point(i)->speed_rpm;
    }

    mec_current[0] = (int16_t)(PID_position_calc(get_mecanum(0),current_speed[0],get_wheel_speed()->wheel_0));
    mec_current[1] = (int16_t)(PID_position_calc(get_mecanum(1),current_speed[1],get_wheel_speed()->wheel_1));
    mec_current[2] = (int16_t)(PID_position_calc(get_mecanum(2),current_speed[2],get_wheel_speed()->wheel_2));
    mec_current[3] = (int16_t)(PID_position_calc(get_mecanum(3),current_speed[3],get_wheel_speed()->wheel_3));

    if(get_remote_control_point()->rc.s[1] == 1)
    {
      CAN_cmd_chassis(0,0,0,0);
    }
    else
    {
      CAN_cmd_chassis(mec_current[0],mec_current[1],mec_current[2],mec_current[3]);
    }
    
}
